package pathfinder;

import java.awt.Graphics;
import java.awt.Polygon;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.ListIterator;
import java.util.Stack;

public class World
{
    private ArrayList<Block> blocks;
    private ArrayList<PointRobot> robots;
    public ArrayList<cell> cellList;
    private int[][] AdjancencyGraph;
    private double speed;
    static private boolean boxOne=false;
    static private boolean boxTwo=false;
    static private boolean boxThree=false;

    public class cell
    {
        public Point topLeft, topRight, bottomLeft,bottomRight;
        public Point midpoint;
        public boolean visited;
        
        public cell(Point left, int width, int height)
        {
            topLeft = left.clone();
            topRight = new Point(topLeft.x+width, topLeft.y);
            bottomLeft = new Point(topLeft.x, topLeft.y+height);
            bottomRight = new Point(topLeft.x+width, topLeft.y+height);
            midpoint = new Point((topLeft.x+topRight.x)/2, (topLeft.y + topRight.y
                    + bottomLeft.y + bottomRight.y)/4);
            visited = false;
        }
        
        public cell(Point leftTop, Point rightTop, Point rightBottom, Point leftBottom)
        {
            topLeft = leftTop.clone();
            topRight = rightTop.clone();
            bottomLeft = leftBottom.clone();
            bottomRight = rightBottom.clone();
            midpoint = new Point((topLeft.x+topRight.x)/2, (topLeft.y + topRight.y
                    + bottomLeft.y + bottomRight.y)/4);
            visited = false;
        }
        
        public boolean contains(Point InputPoint)
        {
           Polygon cellboundary = new Polygon();
           cellboundary.addPoint((int)topLeft.x, (int)topLeft.y);
           cellboundary.addPoint((int)topRight.x, (int)topRight.y);
           cellboundary.addPoint((int)bottomRight.x, (int)bottomRight.y);
           cellboundary.addPoint((int)bottomLeft.x, (int)bottomLeft.y);
           return cellboundary.contains(InputPoint.x, InputPoint.y);
        }
    }

    
    World()
    {
        robots = new ArrayList<>();
        blocks = new ArrayList<>();
        speed = 2;
        
        PointRobot rob = new PointRobot(new Point(5,5), new Point(400,400));
//        rob.addPathPoint(new Point(5,5));
//        rob.addPathPoint(new Point(50,50));
//        rob.addPathPoint(new Point(100,200));
//        rob.addPathPoint(new Point(100,75));
//        rob.addPathPoint(new Point(350,40));
//        rob.addPathPoint(new Point(40,250));
//        rob.addPathPoint(new Point(400,400));
    }

    public void moveAll()
    {
        for(int k = 0; k<robots.size(); k++)
        {
            robots.get(k).move(speed);
        }
    }
    
    public void draw(Graphics g)
    {
        for(int k = 0; k<robots.size(); k++)
        {
            robots.get(k).draw(g);
        }
        for(int k = 0; k<blocks.size(); k++)
        {
            blocks.get(k).draw(g);
        }
    }
    
    public void AddBlock(Point leftCorner, int type)
    {
        blocks.add(new Block(leftCorner,type));
    }
    
    public void newRobot(Point start, Point end)
    {
        //start.x=250;
        //end.x=250;
        PointRobot rob = new PointRobot(start, end);
        rob.calculate(blocks);
        //rob.addPathPoint(start);
        
        //Need to compute path and add all of the points here.
        
        //rob.addPathPoint(end);
        //CalculatePath(rob);
        robots.add(rob);
    }
    
    public void UpdateWorld()
    {
        //Sort list of obstacles in world
        Block.SortBlocks(blocks);
        
        //Initiatilize cell to be entire bounding box of window
        cellList = new ArrayList<cell>();
        cellList.add(new cell(new Point(0,0), MyFrame.WIDTH, MyFrame.HEIGHT));
        
        //Create polygon for every obstacle to obstain line segments and update cells accordingly
        for(Block blockElement: blocks)
        {
            Polygon obstaclePolygon = createPolygon(blockElement);
              
            //Start with the segment that closes the rectangle shape
            Point start = new Point(obstaclePolygon.xpoints[0], obstaclePolygon.ypoints[0]);
            Point end = new Point(obstaclePolygon.xpoints[obstaclePolygon.npoints - 1], obstaclePolygon.ypoints[obstaclePolygon.npoints -1]);
            UpdateCells(start, end);
            
            //Add the remaining three segments of the rectangle
            for (int i = 0; i < obstaclePolygon.npoints - 1; i++)
            {
                start = new Point(obstaclePolygon.xpoints[i], obstaclePolygon.ypoints[i]);
                end = new Point(obstaclePolygon.xpoints[i+1], obstaclePolygon.ypoints[i+1]);
                UpdateCells(start, end);
            }
        }
        
        //Remove cells containing obstacles
        for (Block blockElement: blocks)
        {
            for (int i = 0; i <cellList.size(); i++)
            {
                cell cellElement = cellList.get(i);
                Polygon obstaclPolygon = createPolygon(blockElement);
                if (obstaclPolygon.contains(cellElement.midpoint.x, cellElement.midpoint.y))
                {
                    cellList.remove(i);
                    i--;
                    //cellList.remove(cellElement);
                }
            }
        }
        
        buildAdjanceyGraph();
    }
    
    public void CalculatePath(PointRobot robot)
    {
        int startCell = 0;
        int endCell = 0;
        
        //Scan environment
        UpdateWorld();
        for (int i = 0; i < cellList.size(); i++)
        {
            if (cellList.get(i).contains(robot.getStart()))
                startCell = i;
            if (cellList.get(i).contains(robot.getEnd()))
                endCell = i;
        }
        if (startCell == endCell) //Start and end locations are in same cell
        {
            robot.addPathPoint(robot.getStart());
            robot.addPathPoint(robot.getEnd());
        }
        else
            CalculatePath(robot, startCell, endCell);
    }
    
    public void CalculatePath(PointRobot robot, int startCell, int endCell)
    {
        robot.clearPath();
        int cellsize = cellList.size();
        int[][] tempGraph = new int[cellsize][cellsize];
        
        //Deep copy old adjacency graph -- this is to ensure changes do not affect original
        for (int i = 0; i < cellsize; i++)
        {
            for (int j = 0; j < cellsize; j++)
            {
                tempGraph[i][j] = AdjancencyGraph[i][j];
            }
        }
        
        //Perform breadth first search
        Stack<Integer> searchStack = new Stack<Integer>();
        Stack<Integer> solutionStack = new Stack<Integer>();
        boolean siblingVisited = false;
        
        //Working backwards from endpoint to initial
        int currentNode = endCell;
        searchStack.push(new Integer(currentNode));
        solutionStack.push(new Integer(currentNode));
        do
        {
            currentNode = ((Integer)searchStack.pop()).intValue();
            tempGraph[currentNode][currentNode] = 100;      //Mark node as visited
            
            if (currentNode == startCell)       //Destination reached
            {
                solutionStack.push(new Integer(currentNode));
                ArrayList<Integer> tempPath = new ArrayList<Integer>();
                
                //Add initial robot location to path
                robot.addPathPoint(robot.getStart());
                robot.addPathPoint(cellList.get(startCell).midpoint);
                int prevSolutionNode = startCell, currSolutionNode = 0;
                while (!solutionStack.empty())
                {
                    currSolutionNode = ((Integer) solutionStack.pop()).intValue();
                    cell currCell = cellList.get(currSolutionNode);
                    cell prevCell = cellList.get(prevSolutionNode);
                    cellList.get(currSolutionNode).visited = true;
                    
                    //Get boundary coordinates
                    double boundX = 0, boundY = 0;
                    if (currCell.bottomLeft.x == prevCell.bottomRight.x) //currCell is to the right of previous
                    {
                        boundX = currCell.bottomLeft.x;
                        double boundTop = Math.max(currCell.topLeft.y, prevCell.topRight.y);
                        double boundBottom = Math.min(currCell.bottomLeft.y, prevCell.bottomRight.y);
                        boundY = (boundTop + boundBottom)/2;
                    }
                    else    //currCell is to the left of previous
                    {
                        boundX = currCell.bottomRight.x;
                        double boundTop = Math.max(currCell.topRight.y, prevCell.topLeft.y);
                        double boundBottom = Math.min(currCell.bottomRight.y, prevCell.bottomLeft.y);
                        boundY = (boundTop + boundBottom)/2;
                    }
                    
                    robot.addPathPoint(new Point(boundX, boundY));
                    robot.addPathPoint(cellList.get(currSolutionNode).midpoint);
                    prevSolutionNode = currSolutionNode;
                }
                //Add destination points
                robot.addPathPoint(cellList.get(endCell).midpoint);
                robot.addPathPoint(robot.getEnd());
            }
            else
            {
                siblingVisited = false;
                for (int i = 0; i < cellsize; i++)
                {
                    if (tempGraph[i][currentNode] == 1 && tempGraph[i][i] == 0) //Sibling exists and not visited
                    {
                        siblingVisited = true;
                        searchStack.push(new Integer(i));
                        tempGraph[i][i] = 100;
                    }
                }
                
                if (siblingVisited)
                    solutionStack.push(new Integer(currentNode));
                else    //Find any sibling
                {
                    int searchIndex = ((Integer)searchStack.peek()).intValue();
                    int solutionIndex = 0;
                    do
                    {
                        solutionIndex = ((Integer)solutionStack.pop()).intValue();
                    }while (tempGraph[searchIndex][solutionIndex] == 0);
                    solutionStack.push(new Integer(solutionIndex));
                }
                
            }
        }while(!searchStack.empty());
        System.out.println("No path was found for this robot!");
    }
    
    public void buildAdjanceyGraph()
    {
        int cellcount = cellList.size();
        AdjancencyGraph = new int[cellcount][cellcount];
        
        for (int i  = 0; i < cellcount; i++) 
        {
            for (int j = i+1; j < cellcount; j++) //No self-referential connections
            {
                cell firstCell = cellList.get(i);
                cell secondCell = cellList.get(j);
                
                if (firstCell.bottomRight.x == secondCell.bottomLeft.x) //First cell to left of second
                {
                    if (firstCell.bottomRight.y < secondCell.topLeft.y || firstCell.topRight.y > secondCell.bottomLeft.y)
                    {
                        AdjancencyGraph[i][j] = 0;
                        AdjancencyGraph[j][i] = 0;
                    }
                    else
                    {
                        AdjancencyGraph[i][j] = 1;
                        AdjancencyGraph[j][i] = 1;
                    }
                }
                else if (firstCell.bottomLeft.x == secondCell.bottomRight.x) //First cell to right of second
                {
                    if (secondCell.bottomRight.y < firstCell.topLeft.y || secondCell.topRight.y > firstCell.bottomLeft.y)
                    {
                        AdjancencyGraph[i][j] = 0;
                        AdjancencyGraph[j][i] = 0;
                    }
                    else
                    {
                        AdjancencyGraph[i][j] = 1;
                        AdjancencyGraph[j][i] = 1;
                    }
                }
            }
        }
    }
    
    public Polygon createPolygon(Block InputObstacle)
    {
        Polygon obstaclePolygon = new Polygon();
        Point topLeft = InputObstacle.getLoc().clone();
        Point topRight = new Point(InputObstacle.getLoc().x+InputObstacle.size, InputObstacle.getLoc().y);
        Point bottomRight = new Point(InputObstacle.getLoc().x+InputObstacle.size, InputObstacle.getLoc().y+InputObstacle.size);
        Point bottomLeft = new Point(InputObstacle.getLoc().x, InputObstacle.getLoc().y+InputObstacle.size);

        obstaclePolygon.addPoint((int)topLeft.x, (int)topLeft.y);
        obstaclePolygon.addPoint((int)topRight.x,(int)topRight.y);
        obstaclePolygon.addPoint((int)bottomRight.x, (int)bottomRight.y);
        obstaclePolygon.addPoint((int)bottomLeft.x, (int)bottomLeft.y);
        return obstaclePolygon;
    }
    public void UpdateCells(Point begin, Point end)
    {
        boolean PointEnclosed = false; //Identify if cell contains given point
        if(begin.x == end.x)           //Line segment is vertical
        {
            ArrayList<cell> cells = cellList;
            for(cell element : cells)
            {
                if (PointEnclosed)
                    break;    
                if(element.contains(begin))
                {
                  PointEnclosed = true;
                  cell leftCell = new cell(element.topLeft.clone(), begin.clone(),
                          new Point(begin.x, element.bottomLeft.y), element.bottomLeft.clone());
                  cell rightCell = new cell(leftCell.topRight.clone(), element.topRight.clone(),
                          element.bottomRight.clone(), leftCell.bottomRight.clone());
                  
                  cellList.remove(element);
                  cellList.add(leftCell);
                  cellList.add(rightCell);
                }
            }
        }
        else    //Horizontal line segment
        {
            if (begin.x > end.x)    //Ensure that ordering of points is correct
            {
                Point temp = begin.clone();
                begin = end;
                end = temp;
            }
            ArrayList<cell> cells = new ArrayList<cell>();
            //ArrayList<cell> cells = cellList;
            for (int x = 0; x < cellList.size(); x++)
            {
                cell element = cellList.get(x);
                if ((element.contains(begin)) && (element.contains(end)))
                {
                    Point beginTop = new Point(begin.x, element.topLeft.y);
                    Point endTop = new Point(end.x, element.topLeft.y);
                    Point beginBottom = new Point(begin.x, element.bottomLeft.y);
                    Point endBottom = new Point(end.x, element.bottomLeft.y);
                    
                    cell leftCell = new cell(element.topLeft, beginTop.clone(), beginBottom.clone(), element.bottomLeft);
                    cell topCell = new cell(beginTop.clone(), endTop.clone(), end, begin);
                    cell bottomCell = new cell(begin, end, endBottom, beginBottom);
                    cell rightCell = new cell(endTop, element.topRight, element.bottomRight, endBottom);
                    cells.add(leftCell);
                    cells.add(topCell);
                    cells.add(bottomCell);
                    cells.add(rightCell);
                    cellList.remove(x);
                    x--;
                    //cellList.remove(element);
               }
               else if (element.contains(begin))
               {
                    Point right = new Point(element.bottomRight.x, begin.y);
                    Point top = new Point(begin.x, element.topLeft.y);
                    Point bottom = new Point(begin.x, element.bottomLeft.y);
                    
                    cell leftCell = new cell(element.topLeft, top.clone(), bottom.clone(), element.bottomLeft.clone());
                    cell topCell = new cell(top.clone(), element.topRight.clone(), right.clone(), begin.clone());
                    cell bottomCell = new cell(begin.clone(), right.clone(), element.bottomRight.clone(), bottom.clone());
                    cells.add(leftCell);
                    cells.add(topCell);
                    cells.add(bottomCell);
                    cellList.remove(x);
                    x--;
                    //cellList.remove(element);
               }
               else if (element.contains(end))
               {
                   Point left = new Point(element.bottomLeft.x, end.y);
                   Point top = new Point(end.x, element.topLeft.y);
                   Point bottom = new Point(end.x, element.bottomLeft.y);
                   
                   cell topCell = new cell(element.topLeft.clone(), top.clone(), end.clone(), left.clone());
                   cell bottomCell = new cell(left, end.clone(), bottom.clone(), element.bottomLeft.clone());
                   cell rightCell = new cell(top.clone(), element.topRight.clone(), element.bottomRight.clone(), bottom.clone());
                   cells.add(topCell);
                   cells.add(bottomCell);
                   cells.add(rightCell);
                   cellList.remove(x);
                   x--;
                   //cellList.remove(element);
               }
               else if ((begin.x < element.bottomLeft.x) && (end.x >=element.bottomRight.x))
               {
                   if ((begin.y <= element.bottomLeft.y) && (begin.y >= element.topLeft.y))
                   {
                       Point left = new Point(element.bottomLeft.x, begin.y);
                       Point right = new Point(element.bottomRight.x, begin.y);
                       
                       cell topCell = new cell(element.topLeft.clone(), element.topRight.clone(), right.clone(), left.clone());
                       cell bottomCell = new cell(left.clone(), right.clone(), element.bottomRight.clone(), element.bottomLeft.clone());
                       cells.add(topCell);
                       cells.add(bottomCell);
                       cellList.remove(x);
                       x--;
                       //cellList.remove(element);
                   }
               }
               for(int i = 0; i < cells.size(); i++) //Check if new cells are wide enough
               {
                   cell newelement = cells.get(i);
                   if (newelement.topLeft.x != newelement.topRight.x)
                   {
                       cellList.add(element);
                       System.out.println("Cell size:"+cells.size());
                   }
                   
                       //cellList.add(newelement);
               }
                
               
            }
        }
    }
    
    public void reset()
    {
        blocks.clear();
        robots.clear();
    }
    
    public static double dist(Point a, Point b)
    {
        return Math.sqrt(Math.pow(a.x-b.x,2) + Math.pow(a.y-b.y,2));
    }
    
    public static double direction(Point a, Point b)
    {
        //Returns the angle of a vector defined by origin 'a' and endpoint 'b'
        double result = Math.toDegrees(Math.atan2((a.y-b.y),(a.x-b.x)));
        if(result < 0)
        {
            return result + 360;
        }
        else
        {
            return result;
        }
    }
}
